#include <opencv2/opencv.hpp>
#include <iostream>
#include <cstdio>
#include <cstdlib>
#include <iomanip>  // 用于格式化输出
#include <ctime>    // 用于计时
#include "serial.hpp"
#include "rknn_Detector.hpp"
#include "kalman_filter.hpp"

cv::Mat frame;
KalmanFilter::Kalman1State kXState; // X方向滤波
KalmanFilter::Kalman1State kYState; // Y方向滤波

// 执行系统命令并返回结果
bool executeCommand(const std::string& cmd) 
{
    int result = system(cmd.c_str());
    return (result == 0);
}

// 通用摄像头参数设置函数, 可设置任何通过v4l2-ctl支持的参数
bool setCameraParameter(const std::string& device, const std::string& paramName, int value) 
{
    std::string cmd = "v4l2-ctl -d " + device + " --set-ctrl=" + paramName + "=" + std::to_string(value);
    if (executeCommand(cmd)) 
    {
        // std::cout << "成功设置 " << paramName << " = " << value << std::endl;
        return true;
    } 
    else 
    {
        // std::cerr << "设置 " << paramName << " = " << value << " 失败！" << std::endl;
        return false;
    }
}

// 获取摄像头设备路径（默认/dev/video0）
std::string getCameraDevice() 
{
    // 默认返回/dev/video0
    return "/dev/video0";
}

// 初始化摄像头参数
void initCameraParameters(const std::string& device) 
{
    /* 曝光相关设置 */ 
    setCameraParameter(device, "exposure_auto", 1);               // 1-手动曝光模式 3-自动曝光模式
    setCameraParameter(device, "exposure_absolute", 100);     // 曝光时间（1-5000）
    setCameraParameter(device, "exposure_auto_priority", 0);     // 自动曝光优先级：0-禁用 1-启用
    
    /* 画面质量设置 */
    setCameraParameter(device, "brightness", 0);       // 亮度（-64~64）
    setCameraParameter(device, "contrast", 32);        // 对比度（0~64）
    setCameraParameter(device, "saturation", 60);      // 饱和度（0~128）
    setCameraParameter(device, "hue", 0);      // 色调(-40~40)
    setCameraParameter(device, "gamma", 100);      // 伽马值(72~500)
    setCameraParameter(device, "sharpness", 2);        // 锐度（0~6）

    /* 白平衡设置 */
    setCameraParameter(device, "white_balance_temperature_auto", 1);    // 自动白平衡: 1-启用 0-禁用
    setCameraParameter(device, "white_balance_temperature", 4600);    // 手动白平衡色温(2800~6500)

    /* 增益设置 */
    setCameraParameter(device, "gain", 0);  // 增益(0~100，数值越高画面越亮但噪点越多)
    
    /* 其他实用设置 */ 
    setCameraParameter(device, "power_line_frequency", 1);  //电源频率： 0-不启用 1-50Hz（中国电网），解决闪烁 2-60Hz
    setCameraParameter(device, "backlight_compensation", 1); // 背光补偿：0-禁用 1-中等 2-强
}

// 卡尔曼滤波器
void initKalmanFilters()
{
    KalmanFilter::kalman1Init(&kXState, 0.0f, 100.0f);
    KalmanFilter::kalman1Init(&kYState, 0.0f, 100.0f);

    kXState.q = 0.1f;
    kXState.r = 1.0f;
    kYState.q = 0.1f;
    kYState.r = 1.0f;
}

std::vector<uint8_t> sendInfo(char length = 19, int x = 0, int y = 0, int k = 0, int color = 0)
{
    // 构建发送数据（包含帧头和帧尾）
    std::vector<uint8_t> sendData;
    // 添加帧头
    sendData.push_back(0xFE);
    // 依次添加int类型数据（每个int转换为4字节十六进制）
    auto intToBytes = [](int value) {
        std::vector<uint8_t> bytes;
        bytes.push_back((value >> 24) & 0xFF);  // 高8位
        bytes.push_back((value >> 16) & 0xFF);
        bytes.push_back((value >> 8) & 0xFF);
        bytes.push_back(value & 0xFF);          // 低8位
        return bytes;
    };

    // 添加数据字段
    sendData.push_back(static_cast<uint8_t>(length));  // 直接存入 char 的 8 位值

    auto xBytes = intToBytes(x);
    sendData.insert(sendData.end(), xBytes.begin(), xBytes.end());

    auto yBytes = intToBytes(y);
    sendData.insert(sendData.end(), yBytes.begin(), yBytes.end());

    auto kBytes = intToBytes(k);
    sendData.insert(sendData.end(), kBytes.begin(), kBytes.end());

    auto colorBytes = intToBytes(color);
    sendData.insert(sendData.end(), colorBytes.begin(), colorBytes.end());

    // 添加帧尾
    sendData.push_back(0xFF);

    return sendData;
}

int main()
{    
    // SerialPort seri("/dev/ttyFIQ0", B115200);
    SerialPort serial("/dev/ttyS0", B115200);
    // 启动异步读取线程
    serial.startAsyncRead();

    // 创建检测器实例（指定模型路径和参数）
    // RKNNDetector detector("/home/cat/camera/model/color_pillar320.rknn", 320, 320, 0.7f, 0.45f, 5);
    RKNNDetector detector("/home/cat/camera/model/new.rknn", 320, 320, 0.7f, 0.45f, 8);
    // RKNNDetector detector("/home/cat/camera/model/cross.rknn", 320, 320, 0.7f, 0.45f, 2);
    // 初始化模型
    if (!detector.init()) 
    {
        std::cerr << "Detector init failed" << std::endl;
        return -1;
    }

    initKalmanFilters();

    // 获取摄像头设备路径
    std::string cameraDevice = getCameraDevice();
    // std::cout << "使用摄像头设备：" << cameraDevice << std::endl;
    // 初始化摄像头参数
    initCameraParameters(cameraDevice);
    // 打开摄像头
    cv::VideoCapture cap(cameraDevice);
    if(!cap.isOpened())
    {
        std::cerr << "无法打开摄像头" << std::endl;
        return -1;
    }

    // 设置摄像头分辨率
    cap.set(cv::CAP_PROP_FRAME_WIDTH, 640);
    cap.set(cv::CAP_PROP_FRAME_HEIGHT, 640);
    // 设置目标帧率(硬件最大帧率30)
    int target_fps = 30;
    cap.set(cv::CAP_PROP_FPS, target_fps);
    // std::cout << "目标帧率设置为: " << target_fps << " FPS" << std::endl;
    // 帧率计算相关变量
    double fps = 0.0;
    int frame_count = 0;
    auto start_time = std::chrono::high_resolution_clock::now();
    const int fps_update_interval = 10;  // 每10帧更新一次帧率计算

    // 用于控制发送频率
    auto last_send_time = std::chrono::system_clock::now();
    const int send_interval_seconds = 1;   // 发送间隔(秒)
    // // 设置FFmpeg命令
    // std::string rtmp_url = "rtmp://192.168.1.181:1935/live/livehime";  // 替换为B站的RTMP地址
    // std::string ffmpeg_cmd = "ffmpeg -y -f rawvideo -pix_fmt bgr24 -s 640x480 -r 25 -i - -c:v libx264 -preset ultrafast -maxrate 1000k -bufsize 2000k -pix_fmt yuv420p -f flv " + rtmp_url;
    // // 打开ffmpeg进程
    // FILE *ffmpeg = popen(ffmpeg_cmd.c_str(), "w");
    // if(!ffmpeg)
    // {
    //     std::cerr << "无法打开FFmpeg" << std::endl;
    //     return -1;
    // }

    // for (int i = 0; i < 5; ++i) 
    // {
    //     std::string message = "test " + std::to_string(i) + "\n";
    //     ssize_t bytes_sent = serial.sendString(message);
    //     std::cout << "发送了 " << bytes_sent << " 字节: " << message;
    //     std::this_thread::sleep_for(std::chrono::seconds(2));
    // }
    // // 等待用户输入后退出
    // std::cout << "按Enter键退出..." << std::endl;
    // std::cin.get();

    while(true)
    {
        cap >> frame;   // 获取一帧画面
        if(frame.empty())
        {
            std::cerr << "捕获到空帧" << std::endl;
            break;
        }

        // 获取最新串口状态
         SerialStatus status = serial.getLatestStatus();
        if(status.valid) // status.valid
        {
            if(true) // status.flag == 0x11 true
            {
                // 推理并获取检测结果
                std::vector<Bbox> boxes = detector.process(frame);
                // 检查是否达到发送时间间隔
                auto current_time_send = std::chrono::system_clock::now();
                std::chrono::duration<double> elapsed0 = current_time_send - last_send_time;

                if(20 * elapsed0.count() >= send_interval_seconds)
                {
                    last_send_time = current_time_send;                
                    for(auto b : boxes)
                    {
                        auto x_center = (b.x2 - b.x1) / 2 + b.x1;
                        auto y_center = (b.y2 - b.y1) / 2 + b.y1;
                        x_center = KalmanFilter::kalman1Filter(&kXState, x_center);
                        y_center = KalmanFilter::kalman1Filter(&kYState, y_center);
                        std::cout << "x中心坐标：" << x_center << std::endl;
                        std::cout << "y中心坐标：" << y_center << std::endl;
                        int send_x, send_y;
                        ssize_t bytesSent;
                        std::vector<uint8_t> send_info;

                        if(status.flag == 0x01 && b.cls != 0 && b.cls != 6 && b.cls != 7) // 物块信息
                        {
                            // 发送数据
                            send_x = static_cast<int>(x_center - 330 + 0.5f);
                            send_y = static_cast<int>(y_center - 297 + 0.5f);
                            send_info = sendInfo(19, send_x, send_y, 0, b.cls);
                            bytesSent = serial.sendBytes(send_info);
                            std::cout << " 发送物块信息(颜色和坐标) " << std::endl;
                        }
                        else if(status.flag == 0x02 && b.cls == 7)    // 场地圆(校准车身)
                        {
                            // 发送数据
                            send_x = static_cast<int>(x_center - 330 + 0.5f);
                            send_y = static_cast<int>(y_center - 287 + 0.5f);
                            send_info = sendInfo(19, send_x, send_y, 0, b.cls);
                            bytesSent = serial.sendBytes(send_info);
                            std::cout << " 发送场地圆 " << std::endl;
                        }
                        else if(status.flag == 0x03 && b.cls == 6)  // 靶心
                        {
                            // 发送数据
                            send_x = static_cast<int>(x_center - 330 + 0.5f);
                            send_y = static_cast<int>(y_center - 290 + 0.5f);
                            send_info = sendInfo(19, send_x, send_y, 0, b.cls);
                            bytesSent = serial.sendBytes(send_info);
                            std::cout << " 发送靶心圆 " << std::endl;
                        }
                        else if(b.cls == 0)
                        {
                            send_x = 0;
                            send_y = 0;
                            send_info = sendInfo(19, send_x, send_y, 0, b.cls);
                            bytesSent = serial.sendBytes(send_info);
                            std::cout << " 没有目标信息 " << std::endl;
                        }
                        std::cout << "send_x: " << send_x << std::endl;
                        std::cout << "send_y: " << send_y << std::endl;
                        std::cout << "发送 " << bytesSent << " 字节：";
                        for(uint8_t b : send_info)
                        {
                            std::cout << std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(b) << " ";
                        }
                        std::cout << std:: dec << std::endl;

                        send_x = 0;
                        send_y = 0;

                        std::cout << "标志位: 0x" << std::hex <<std::setw(2) << std::setfill('0') << static_cast<int>(status.flag) << std::dec << std::endl;
                    }   
                }

                // 绘制结果
                detector.draw(frame, boxes);
            }
            else if(status.flag == 0x01)
            {
                sendInfo(19, 0, 0, 0, 0);
            }
        }

        // 计算帧率
        frame_count++;
        if (frame_count % fps_update_interval == 0) 
        {
            auto current_time = std::chrono::high_resolution_clock::now();
            std::chrono::duration<double> elapsed = current_time - start_time;
            fps = frame_count / elapsed.count();  // 总帧数 / 总时间 = 帧率
            
            // 重置计数器，避免数值过大
            frame_count = 0;
            start_time = current_time;
        }

        // 在画面上绘制帧率信息 
        std::string fps_text = "FPS: " + std::to_string(static_cast<int>(fps));
        cv::putText(frame, fps_text, cv::Point(10, 30), cv::FONT_HERSHEY_SIMPLEX, 1.0, cv::Scalar(0, 255, 0), 2);

        // 将帧写入进程
        // fwrite(frame.data, 1, frame.total() * frame.elemSize(), ffmpeg);

        // 显示画面(给自己看的)
        cv::imshow("Camera", frame);
        if(cv::waitKey(1) == 27)    // 按下ESC退出
        {
            break;
        }
    }

    // 停止读取线程
    serial.stop();
    // 释放资源
    cap.release();
    cv::destroyAllWindows();
    // pclose(ffmpeg);

    return 0;
}
